#include "quaternionmath.h"

quaternionMath::quaternionMath()
{

}

quaternionMath::~quaternionMath()
{

}


// 创建用于旋转的四元数q, v必须为单位向量
void quaternionMath::QuatCreate(QUAT_PTR q, QVector3D &v, double theta)
{
    double theta_div_2 = (0.5)*theta;
    double sin_theta = sin(theta_div_2);

    q->quat.x = sin_theta * v.x();
    q->quat.y = sin_theta * v.y();
    q->quat.z = sin_theta * v.z();
    q->quat.w = cos( theta_div_2 );
}

void quaternionMath::QuatGetVectorAndTheta(QUAT_PTR q, QVector3D &v, double *theta)
{
    *theta = acos(q->quat.w);
    double sin_theta_inv = 1.0/sin(*theta);

    v.setX(q->quat.x * sin_theta_inv);
    v.setY(q->quat.y * sin_theta_inv);
    v.setZ(q->quat.z * sin_theta_inv);

    *theta *= 2;
}

void quaternionMath::QuatAdd(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum)
{
    qsum->quat.x = q1->quat.x + q2->quat.x;
    qsum->quat.y = q1->quat.y + q2->quat.y;
    qsum->quat.z = q1->quat.z + q2->quat.z;
    qsum->quat.w = q1->quat.w + q2->quat.w;
}

void quaternionMath::QuatSub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff)
{
    qdiff->quat.x = q1->quat.x - q2->quat.x;
    qdiff->quat.y = q1->quat.y - q2->quat.y;
    qdiff->quat.z = q1->quat.z - q2->quat.z;
    qdiff->quat.w = q1->quat.w - q2->quat.w;
}

void quaternionMath::QuatConjugate(QUAT_PTR q, QUAT_PTR qconj)
{
    qconj->quat.x = -q->quat.x;
    qconj->quat.y = -q->quat.y;
    qconj->quat.z = -q->quat.z;
    qconj->quat.w = q->quat.w;
}

void quaternionMath::QuatScale(QUAT_PTR q, double scale, QUAT_PTR qs)
{
    qs->quat.x = scale * q->quat.x;
    qs->quat.y = scale * q->quat.y;
    qs->quat.z = scale * q->quat.z;
    qs->quat.w = scale * q->quat.w;
}

double quaternionMath::QuatNorm(QUAT_PTR q)
{
    return sqrt(q->quat.w * q->quat.w + q->quat.x * q->quat.x + q->quat.y * q->quat.y + q->quat.z * q->quat.z);
}

double quaternionMath::QuatNorm2(QUAT_PTR q)
{
    return q->quat.w * q->quat.w + q->quat.x * q->quat.x + q->quat.y * q->quat.y + q->quat.z * q->quat.z;
}

void quaternionMath::QuatNormalize(QUAT_PTR q, QUAT_PTR qn)
{
    double qlength_inv = 1.0/(sqrt(q->quat.w*q->quat.w + q->quat.x*q->quat.x + q->quat.y*q->quat.y + q->quat.z*q->quat.z));

    qn->quat.w = q->quat.w * qlength_inv;
    qn->quat.x = q->quat.x * qlength_inv;
    qn->quat.y = q->quat.y * qlength_inv;
    qn->quat.z = q->quat.z * qlength_inv;
}

void quaternionMath::QuatUnitInverse(QUAT_PTR q, QUAT_PTR qi)
{
    qi->quat.w =  q->quat.w;
    qi->quat.x = -q->quat.x;
    qi->quat.y = -q->quat.y;
    qi->quat.z = -q->quat.z;
}

void quaternionMath::QuatInverse(QUAT_PTR q, QUAT_PTR qi)
{
    double norm2_inv = 1.0 / (q->quat.w * q->quat.w + q->quat.x * q->quat.x + q->quat.y * q->quat.y + q->quat.z * q->quat.z);

    qi->quat.w =  q->quat.w * norm2_inv;
    qi->quat.x = -q->quat.x * norm2_inv;
    qi->quat.y = -q->quat.y * norm2_inv;
    qi->quat.z = -q->quat.z * norm2_inv;
}

void quaternionMath::QuatMul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod)
{
    double prd_0 = (q1->quat.z - q1->quat.y) * (q2->quat.y - q2->quat.z);
    double prd_1 = (q1->quat.w + q1->quat.x) * (q2->quat.w + q2->quat.x);
    double prd_2 = (q1->quat.w - q1->quat.x) * (q2->quat.y + q2->quat.z);
    double prd_3 = (q1->quat.y + q1->quat.z) * (q2->quat.w - q2->quat.x);
    double prd_4 = (q1->quat.z - q1->quat.x) * (q2->quat.x - q2->quat.y);
    double prd_5 = (q1->quat.z + q1->quat.x) * (q2->quat.x + q2->quat.y);
    double prd_6 = (q1->quat.w + q1->quat.y) * (q2->quat.w - q2->quat.z);
    double prd_7 = (q1->quat.w - q1->quat.y) * (q2->quat.w + q2->quat.z);
    double prd_8 = prd_5 + prd_6 + prd_7;
    double prd_9 = 0.5 * (prd_4 + prd_8);

    qprod->quat.w = prd_0 + prd_9 - prd_5;
    qprod->quat.x = prd_1 + prd_9 - prd_8;
    qprod->quat.y = prd_2 + prd_9 - prd_7;
    qprod->quat.z = prd_3 + prd_9 - prd_6;
}

void quaternionMath::QuatMul3(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod)
{
    QUAT_PTR qtmp;
    QuatMul(q1, q2, qtmp);
    QuatMul(qtmp, q3, qprod);
    qDebug()<<"qtmp:"<<qtmp;
}
